#include "StdAfx.h"
#include "DrawOpticalFlowTrack.h"


CDrawOpticalFlowTrack::CDrawOpticalFlowTrack(void)
{
	//image  = 0;
	grey = 0, prev_grey = 0, pyramid = 0, prev_pyramid = 0;
	points[0] = new CvPoint2D32f();
	points[0]->x = 0;
	points[0]->y = 0;
	points[1] = new CvPoint2D32f();
	points[1]->x = 0;
	points[1]->y = 0;
	MAX_COUNT = 500;
	status = 0;
	flags = 0;
	count = 0;
	win_size = 10;
	add_remove_pt = 0;
}


CDrawOpticalFlowTrack::~CDrawOpticalFlowTrack(void)
{
	//if(image != NULL)
	//{
	//	cvReleaseImage(&image);
	//}
	if(grey != NULL)
	{
		cvReleaseImage(&grey);
	}
	if(prev_grey != NULL)
	{
		cvReleaseImage(&prev_grey);
	}
	if(pyramid != NULL)
	{
		cvReleaseImage(&pyramid);
	}
	if(prev_pyramid != NULL)
	{
		cvReleaseImage(&prev_pyramid);
	}
	if(swap_temp != NULL)
	{
		cvReleaseImage(&swap_temp);
	}
	delete [] points;
	delete swap_points;
	
}
BEGIN_MESSAGE_MAP(CDrawOpticalFlowTrack, CDrawOpenCV)
	ON_WM_PAINT()
	ON_WM_KEYDOWN()
END_MESSAGE_MAP()




void CDrawOpticalFlowTrack::DealwithFrame()
{
	// TODO: Add your command handler code here
	if(m_pFrame == NULL)
	{			
		return ;
	}
	if(m_pShow == NULL)
	{	
        m_pShow = cvCreateImage( cvGetSize(m_pFrame), 8, 3 );
        m_pShow->origin = m_pFrame->origin;
        grey = cvCreateImage( cvGetSize(m_pFrame), 8, 1 );
        prev_grey = cvCreateImage( cvGetSize(m_pFrame), 8, 1 );
        pyramid = cvCreateImage( cvGetSize(m_pFrame), 8, 1 );
        prev_pyramid = cvCreateImage( cvGetSize(m_pFrame), 8, 1 );
        points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
        points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
        status = (char*)cvAlloc(MAX_COUNT);
        flags = 0;
	}
	
    cvCopy( m_pFrame, m_pShow, 0 );
    cvCvtColor( m_pShow, grey, CV_BGR2GRAY );
	int i, k, c;
    //if( night_mode )
    //    cvZero( image );
    //    
    //if( need_to_init )
    {
        /* automatic initialization */
        IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 );
        IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 );
        double quality = 0.01;
        double min_distance = 10;

        count = MAX_COUNT;
        cvGoodFeaturesToTrack( grey, eig, temp, points[1], &count,
                                quality, min_distance, 0, 3, 0, 0.04 );
        cvFindCornerSubPix( grey, points[1], count,
            cvSize(win_size,win_size), cvSize(-1,-1),
            cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
        cvReleaseImage( &eig );
        cvReleaseImage( &temp );
        add_remove_pt = 0;
    }
    //else if( count > 0 )
    //{
        //cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
        //    points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0,
        //    cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
        //flags |= CV_LKFLOW_PYR_A_READY;
        for( i = k = 0; i < count; i++ )
        {
            if( add_remove_pt )
            {
                double dx = pt.x - points[1][i].x;
                double dy = pt.y - points[1][i].y;
                if( dx*dx + dy*dy <= 25 )
                {
                    add_remove_pt = 0;
                    continue;
                }
            }                
            if( !status[i] )
                continue;
                
            points[1][k++] = points[1][i];
            cvCircle( m_pShow, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
        }
        //count = k;
    //}

    //if( add_remove_pt && count < MAX_COUNT )
    //{
    //    points[1][count++] = cvPointTo32f(pt);
    //    cvFindCornerSubPix( grey, points[1] + count - 1, 1,
    //        cvSize(win_size,win_size), cvSize(-1,-1),
    //        cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
    //    add_remove_pt = 0;
    //}

    CV_SWAP( prev_grey, grey, swap_temp );
    CV_SWAP( prev_pyramid, pyramid, swap_temp );
    CV_SWAP( points[0], points[1], swap_points );
    //need_to_init = 0;
    //cvShowImage( "LkDemo", image );

    //c = cvWaitKey(10);
    //if( (char)c == 27 )
    //    break;
    //switch( (char) c )
    //{
    //case 'r':
    //    need_to_init = 1;
    //    break;
    //case 'c':
    //    count = 0;
    //    break;
    //case 'n':
    //    night_mode ^= 1;
    //    break;
    //default:
    //    ;
    //}


}


void CDrawOpticalFlowTrack::OnKeyDown(UINT nChar, UINT nRepCnt, UINT nFlags)
{
	// TODO: Add your message handler code here and/or call default
	int i = 0;
	CDrawOpenCV::OnKeyDown(nChar, nRepCnt, nFlags);
}


BOOL CDrawOpticalFlowTrack::PreTranslateMessage(MSG* pMsg)
{
	// TODO: Add your specialized code here and/or call the base class
	if(pMsg->message==WM_KEYDOWN)   
	{   
		int i = 0;
	}
	return CDrawOpenCV::PreTranslateMessage(pMsg);
}
